/*************************************************************
*
* M3D_POLY.CPP
*
* Polygon / plane processing functions
* 'VNPlane' - plane specified by a point on a plane ('V')
*             and plane normal ('N')
* 'Plane'   - plane specified by three points
*************************************************************/
#include <stdafx.h>

//#include <ap_comm.h>
#include <m3d/m3d.h>

/*************************************************************
* m3dGetPolyEdge()
*
*************************************************************/
void m3dGetPolyEdge (const m3dPOLY *poly, int indEdge, m3dV *lB, m3dV *lE)
{
   ASSERT(indEdge >= 0 && indEdge < m3dPolyNVert(poly));

   *lB = *m3dPolyVertex(poly, indEdge);
   *lE = *m3dPolyVertex(poly, (indEdge+1) % m3dPolyNVert(poly));

   return;
}

/*************************************************************
* m3dIsEqualPoly()
*
* IN : poly1, poly2 - two polygons to compare
*      acc   - accuracy to compare points forming the polygon       
* RET: TRUE  - if polygons have the same number of vertices
*              and their coords are the same with the accuracy 'acc'
*      FALSE - otherwise
*
* Compare two 3D polygons
*************************************************************/
BOOL m3dIsEqualPoly_A (m3dPOLY *poly1, m3dPOLY *poly2, float acc)
{
   int i;

   // check the number of vertices in polygons
   ASSERT(m3dPolyNVert(poly1) >= 3 && m3dPolyNVert(poly1) < M3D_MAX_NVERT);
   ASSERT(m3dPolyNVert(poly2) >= 3 && m3dPolyNVert(poly2) < M3D_MAX_NVERT);

   if (m3dPolyNVert(poly1) != m3dPolyNVert(poly2))  {
      return FALSE;
   }

   for (i = 0; i < m3dPolyNVert(poly1); i++)  {
      if (!m3dIsEqualPoint_A(m3dPolyVertex(poly1,i), m3dPolyVertex(poly2,i), acc))  {
         return FALSE;
      }
   }

   return TRUE;
}

/*************************************************************
* m2dIsEqualPoly()
*
* IN:  poly1, poly2 - two polygons to compare
*      acc   - accuracy to compare points forming the polygon       
* RET: TRUE  - if polygons have the same number of vertices
*              and their coords are the same with the accuracy 'acc'
*      FALSE - otherwise
*
* Compare two 2D polygons
*************************************************************/
BOOL m2dIsEqualPoly_A (m2dPOLY *poly1, m2dPOLY *poly2, float acc)
{
   int i;

   // check the number of vertices in polygons
   ASSERT(m3dPolyNVert(poly1) >= 3 && m3dPolyNVert(poly1) < M3D_MAX_NVERT);
   ASSERT(m3dPolyNVert(poly2) >= 3 && m3dPolyNVert(poly2) < M3D_MAX_NVERT);

   if (m3dPolyNVert(poly1) != m3dPolyNVert(poly2))  {
      return FALSE;
   }

   for (i = 0; i < m3dPolyNVert(poly1); i++)  {
      if (!m2dIsEqualPoint_A(m3dPolyVertex(poly1,i),m3dPolyVertex(poly2,i),acc))  {
         return FALSE;
      }
   }

   return TRUE;
}

/*************************************************************
* m3xyPoly3DToPoly2D()
*
* IN:  poly3D - polygon to be converted
* OUT: poly2D - converted polygon
*
* NOTE: (X, Y, Z) -> (X, Y) (polygon may become re-oriented)
*
* Convert polygon represenations
*************************************************************/
void m3xyPoly3DToPoly2D (m3dPOLY *poly3D, m2dPOLY *poly2D)
{
   int i;

   // check the number of vertices in input polygon
   ASSERT(m3dPolyNVert(poly3D) >= 3 && m3dPolyNVert(poly3D) < M3D_MAX_NVERT);

   for (i = 0; i < m3dPolyNVert(poly3D); i++ )  {
      m3dPolyVertexX(poly2D,i) = m3dPolyVertexX(poly3D,i);
      m3dPolyVertexY(poly2D,i) = m3dPolyVertexY(poly3D,i);
   }
   m3dPolyNVert(poly2D) = m3dPolyNVert(poly3D);
   return;
}

/*************************************************************
* m3xzPoly3DToPoly2D()
*
* IN:  poly3D - polygon to be converted
* OUT: poly2D - converted polygon
*
* NOTE: (X, Y, Z) -> (X, Z) (polygon may become re-oriented)
*
* Convert polygon represenations
*************************************************************/
void m3xzPoly3DToPoly2D (m3dPOLY *poly3D, m2dPOLY *poly2D)
{
   int i;

   // check the number of vertices in input polygon
   ASSERT(m3dPolyNVert(poly3D) >= 3 && m3dPolyNVert(poly3D) < M3D_MAX_NVERT);

   for (i = 0; i < m3dPolyNVert(poly3D); i++ )  {
      m3dPolyVertexX(poly2D,i) = m3dPolyVertexX(poly3D,i);
      m3dPolyVertexY(poly2D,i) = m3dPolyVertexZ(poly3D,i);
   }
   m3dPolyNVert(poly2D) = m3dPolyNVert(poly3D);
   return;
}

/*************************************************************
* m3yzPoly3DToPoly2D()
*
* IN:  poly3D - polygon to be converted
* OUT: poly2D - converted polygon
*
* NOTE: (X, Y, Z) -> (Y, Z) (polygon may become re-oriented)
*
* Convert polygon represenations
*************************************************************/
void m3yzPoly3DToPoly2D (m3dPOLY *poly3D, m2dPOLY *poly2D)
{
   int i;

   // check the number of vertices in input polygon
   ASSERT(m3dPolyNVert(poly3D) >= 3 && m3dPolyNVert(poly3D) < M3D_MAX_NVERT);

   for (i = 0; i < m3dPolyNVert(poly3D); i++ )  {
      m3dPolyVertexX(poly2D,i) = m3dPolyVertexY(poly3D,i);
      m3dPolyVertexY(poly2D,i) = m3dPolyVertexZ(poly3D,i);
   }
   m3dPolyNVert(poly2D) = m3dPolyNVert(poly3D);
   return;
}
/*******************************************************
* m3xyPoly2DToPoly3D()
*
* Convert polygon representation (X, Y) -> (X, Y, 0)
*
* IN : poly2D
* OUT: poly3D
*******************************************************/
void m3xyPoly2DToPoly3D (m2dPOLY *poly2D, m3dPOLY *poly3D )
{
   int i;

   m3dPolyNVert(poly3D) = m3dPolyNVert(poly2D);
   for (i=0; i<m3dPolyNVert(poly3D); i++) {
      m3dPolyVertexX(poly3D, i) = m3dPolyVertexX(poly2D, i);
      m3dPolyVertexY(poly3D, i) = m3dPolyVertexY(poly2D, i);
      m3dPolyVertexZ(poly3D, i) = 0.f;
   }
   return;
}
/*******************************************************
* m3xzPoly2DToPoly3D()
*
* Convert polygon representation (X, Y) -> (X, 0, Y)
*
* IN : poly2D
* OUT: poly3D
*******************************************************/
void m3xzPoly2DToPoly3D (m2dPOLY *poly2D, m3dPOLY *poly3D )
{
   int i;

   m3dPolyNVert(poly3D) = m3dPolyNVert(poly2D);
   for (i=0; i<m3dPolyNVert(poly3D); i++) {
      m3dPolyVertexX(poly3D, i) = m3dPolyVertexX(poly2D, i);
      m3dPolyVertexY(poly3D, i) = 0.f;
      m3dPolyVertexZ(poly3D, i) = m3dPolyVertexY(poly2D, i);
   }
   return;
}

/*******************************************************
* m3yzPoly2DToPoly3D()
* convert polygon representation (X, Y) -> (0, X, Y)
* IN : poly2D
* OUT: poly3D
* RET: none
*******************************************************/
void m3yzPoly2DToPoly3D (m2dPOLY *poly2D, m3dPOLY *poly3D )
{
   int i;

   m3dPolyNVert(poly3D) = m3dPolyNVert(poly2D);
   for (i=0; i<m3dPolyNVert(poly3D); i++) {
      m3dPolyVertexX(poly3D, i) = 0.f;
      m3dPolyVertexY(poly3D, i) = m3dPolyVertexX(poly2D, i);
      m3dPolyVertexZ(poly3D, i) = m3dPolyVertexY(poly2D, i);
   }
   return;
}

/*************************************************************
* m3dGetPolyNormal()
*
* IN:  poly  - polygon, specifying the plane (by its first three points)
* OUT: norm  - normal
* RET: TRUE  - normal was successfully computed
*      FALSE - cannot compute normal, since polygon is degenerate
*
* NOTE: if normal cannot be computed, FALSE is returned
*       (ASSERT() is not generated)
*
* Calculate normal to poly/plane
*************************************************************/
void m3dGetPolyNormal (m3dPOLY *poly, m3dV *norm)
{
   m3dV v1, v2;

   // check the number of vertices in input polygon
   ASSERT(m3dPolyNVert(poly) >= 3 && m3dPolyNVert(poly) < M3D_MAX_NVERT);

   m3dSubtractVector(m3dPolyVertex(poly,1),m3dPolyVertex(poly,0),&v1);
   m3dSubtractVector(m3dPolyVertex(poly,2),m3dPolyVertex(poly,1),&v2);
   m3dCrossProduct(&v1,&v2,norm);
   m3dNormalize(norm);

   return ;
}
/*************************************************************
* _m3dGetPolyNormal ()
*
* IN:  poly  - polygon, specifying the plane (by its first three points)
* OUT: norm  - normal
* RET: TRUE  - normal was successfully computed
*      FALSE - cannot compute normal, since polygon is degenerate
*
* NOTE: if normal cannot be computed, FALSE is returned
*       (ASSERT() is not generated)
*
* Calculate normal to poly/plane
*************************************************************/
BOOL _m3dGetPolyNormal (m3dPOLY *poly, m3dV *norm)
{
   m3dV v1, v2;

   // check the number of vertices in input polygon
   ASSERT(m3dPolyNVert(poly) >= 3 && m3dPolyNVert(poly) < M3D_MAX_NVERT);

   m3dSubtractVector(m3dPolyVertex(poly,1),m3dPolyVertex(poly,0),&v1);
   m3dSubtractVector(m3dPolyVertex(poly,2),m3dPolyVertex(poly,1),&v2);
   m3dCrossProduct(&v1,&v2,norm);
   if (!_m3dNormalize(norm)) {
      return FALSE;
   }

   return TRUE;
}
/*************************************************************
* m3dGetPolyNormal ()
*
* Calculate normal to triangle
*************************************************************/
void m3dGetTriNormal (m3dTRI *tri, m3dV *norm)
{
   m3dV v1, v2;

   m3dSubtractVector(tri->GetVert(1), tri->GetVert(0), &v1);
   m3dSubtractVector(tri->GetVert(2), tri->GetVert(1), &v2);
   m3dCrossProduct(&v1,&v2,norm);
   m3dNormalize(norm);
   return;
}
/*************************************************************
* _m3dGetPolyNormal ()
*
* Calculate normal to triangle
*************************************************************/
BOOL _m3dGetTriNormal (m3dTRI *tri, m3dV *norm)
{
   m3dV v1, v2;

   m3dSubtractVector(tri->GetVert(1), tri->GetVert(0), &v1);
   m3dSubtractVector(tri->GetVert(2), tri->GetVert(1), &v2);
   m3dCrossProduct(&v1,&v2,norm);
   if (!_m3dNormalize(norm)) {
      return FALSE;
   }

   return TRUE;
}


/*************************************************************
* m3dGetPolyPerpVector()
*
* IN:  poly - polygon, specifying the plane (by its first three points)
* OUT: norm - perpendicular vector to the specified plane
*
* Calculate a vector that is collinear to normal one, but may
* have non-unit length (this saves up m3dNormalize() call
*************************************************************/
void __CDECL m3dGetPolyPerpVector (m3dPOLY *poly, m3dV *norm)
{
   ASSERT(m3dPolyNVert(poly) >= 3);

   ASSERT(sizeof(poly->vert[0]) == 12);

#if defined M3D_USE_SSE
   __asm {
      // TODO: toss it
      mov         eax, poly
      movss       xmm2, dword ptr [eax+ 0]m3dPOLY.vert
      movss       xmm0, dword ptr [eax+12]m3dPOLY.vert
      subss       xmm0, xmm2
      movss       xmm6, dword ptr [eax+ 4]m3dPOLY.vert
      movss       xmm1, dword ptr [eax+16]m3dPOLY.vert
      subss       xmm1, xmm6
      movss       xmm3, dword ptr [eax+ 8]m3dPOLY.vert
      movss       xmm4, dword ptr [eax+20]m3dPOLY.vert
      subss       xmm4, xmm3
      movss       xmm5, dword ptr [eax+24]m3dPOLY.vert
      subss       xmm5, xmm2
      movss       xmm7, dword ptr [eax+28]m3dPOLY.vert
      subss       xmm7, xmm6
      movss       xmm6, dword ptr [eax+32]m3dPOLY.vert
      subss       xmm6, xmm3
      movss       xmm2, xmm4
      mov         eax, norm
      mulss       xmm2, xmm7
      movss       xmm3, xmm1
      mulss       xmm3, xmm6
      subss       xmm3, xmm2
      mulss       xmm4, xmm5
      movss       [eax], xmm3
      mulss       xmm6, xmm0
      subss       xmm4, xmm6
      movss       [eax+4], xmm4
      mulss       xmm1, xmm5
      mulss       xmm0, xmm7
      subss       xmm0, xmm1
      movss       [eax+8], xmm0
   }

#else
   m3dV v1, v2;
   m3dSubtractVector(m3dPolyVertex(poly,1),m3dPolyVertex(poly,0),&v1);
   m3dSubtractVector(m3dPolyVertex(poly,2),m3dPolyVertex(poly,1),&v2);
   m3dCrossProduct(&v1,&v2,norm);
#endif
}

/*************************************************************
* m3dGetPlaneNormal()
*
* IN:  pV1, pV2, pV3 - three points specifying the plane
* OUT: norm          - normal
*
* NOTE: if normal cannot be computed, ASSERT() is generated
*
* Calculate normal to poly/plane
*************************************************************/
void m3dGetPlaneNormal (m3dV *pV1, m3dV *pV2, m3dV *pV3, m3dV *norm)
{
   m3dV v1, v2;

   m3dSubtractVector(pV2,pV1,&v1);
   m3dSubtractVector(pV3,pV2,&v2);
   m3dCrossProduct(&v1,&v2,norm);
   m3dNormalize(norm);

   return;
}

/*************************************************************
* m3dGetPlanePerpVector()
*
* IN:  pV1, pV2, pV3 - three points specifying the plane
* OUT: norm          - perpendicular vector
*
* NOTE: if normal cannot be computed, ASSERT() is generated
*
* Calculate a vector that is collinear to normal one, but may
* have non-unit length (this saves up m3dNormalize() call
*************************************************************/
void __CDECL m3dGetPlanePerpVector (m3dV *pV1, m3dV *pV2, m3dV *pV3, m3dV *norm)
{
#if defined M3D_USE_SSE
   __asm {
      mov         eax, pV1
      mov         ebx, pV2
      movss       xmm2, dword ptr [eax]         // 2  = x0
      mov         ecx, pV3
      movss       xmm0, dword ptr [ebx]         // 0  = x1
      subss       xmm0, xmm2                    // 0  = x1-x0
      movss       xmm6, dword ptr [eax+4]       // 6  = y0
      movss       xmm1, dword ptr [ebx+4]       // 1  = y1
      subss       xmm1, xmm6                    // 1  = y1-y0
      movss       xmm3, dword ptr [eax+8]       // 3  = z0
      movss       xmm4, dword ptr [ebx+8]       // 4  = z1
      mov         eax, norm
      subss       xmm4, xmm3                    // 4  = z1-z0
      movss       xmm5, dword ptr [ecx]         // 5  = x2
      subss       xmm5, xmm2                    // 5  = x2-x0
      movss       xmm7, dword ptr [ecx+4]       // 7  = y2
      subss       xmm7, xmm6                    // 7  = y2-y0
      movss       xmm6, dword ptr [ecx+8]       // 6  = z2
      subss       xmm6, xmm3                    // 6  = z2-z0
      movss       xmm2, xmm4                    // 2  = z1-z0
      mulss       xmm2, xmm7                    // 2  = (z1-z0)*(y2-y0) 
      movss       xmm3, xmm1                    // 3  = y1-y0
      mulss       xmm3, xmm6                    // 3  = (y1-y0)*(z2-z0)
      subss       xmm3, xmm2                    // 3  = X
      mulss       xmm4, xmm5                    // 4  = (z1-x0)*(x2-x0)
      movss       [eax], xmm3
      mulss       xmm6, xmm0                    // 6  = (z2-z0)*(x1-x2)
      subss       xmm4, xmm6                    // 6  = Y
      movss       [eax+4], xmm4
      mulss       xmm1, xmm5                    // 1  = (y1-y0)*(x2-x0)
      mulss       xmm0, xmm7                    // 0  = (x1-x0)*(y2-y0)
      subss       xmm0, xmm1                    // 0  = Z
      movss       [eax+8], xmm0
   }

#else
   m3dV v1, v2;
   m3dSubtractVector(pV2,pV1,&v1);
   m3dSubtractVector(pV3,pV2,&v2);
   m3dCrossProduct(&v1,&v2,norm);
#endif
}

/*************************************************************
* m3dDistPointPlane()
* m3dDistPointPlane_S()
*
* IN:  pV3D   - point in 3D
*      poly3D - polygon defining the plane
*      norm   - normal to the plane (polygon)
* RET: distance from point to the plane
*
* Calculate the distance between plane (specified by a polygon)
* and a given point in space
* dist = FAbs(norm*pV3D - norm*Vert0) = FAbs(norm*(pV3D - Vert0))
*************************************************************/
float m3dDistPointPlane (const m3dV *pV3D, m3dPOLY *poly3D, m3dV *norm)
{
   float    dist;
   m3dV     v;
   m3dV     normTmp;

   if (norm == NULL) {
      norm = &normTmp;
      m3dGetPolyNormal(poly3D, norm);
   }

   // check that normal is indeed normal
   ASSERT(m3dIsZero_A(m3dLengthVector(norm)-1.f,10*M3D_EPSILON));

   m3dSubtractVector(pV3D, m3dPolyVertex(poly3D,0), &v);
   dist = m3dDotProduct(&v, norm);
   return m3dFAbs(dist);
}

float m3dDistPointPlane_S (m3dV *pV3D, m3dPOLY *poly3D, m3dV *norm)
{
   float    dist;
   m3dV     v;
   m3dV     normTmp;

   if (norm == NULL) {
      norm = &normTmp;
      m3dGetPolyNormal(poly3D, norm);
   }

   // check that normal is indeed normal
   ASSERT(m3dIsZero_A(m3dLengthVector(norm)-1.f,10*M3D_EPSILON));

   m3dSubtractVector(pV3D, m3dPolyVertex(poly3D,0), &v);
   dist = m3dDotProduct(&v, norm);
   return dist;
}

/*************************************************************
* m3dGetPolyBBox()
*
* Compute the poly3D bounding box
*
* IN:  poly      - input polygon
* OUT: bll, fur  - bbox corners
*************************************************************/
void m3dGetPolyBBox (m3dPOLY *poly, m3dV *bll, m3dV *fur)
{
   int i;

   bll->x = bll->y = bll->z =  M3D_INFINITY;
   fur->x = fur->y = fur->z = -M3D_INFINITY;

   for (i = 0; i < poly->nVert; i++ )  {
      if (bll->x > m3dPolyVertexX(poly,i) )  {
         bll->x = m3dPolyVertexX(poly,i);
      }
      if (bll->y > m3dPolyVertexY(poly,i) )  {
         bll->y = m3dPolyVertexY(poly,i);
      }
      if (bll->z > m3dPolyVertexZ(poly,i) )  {
         bll->z = m3dPolyVertexZ(poly,i);
      }

      if (fur->x < m3dPolyVertexX(poly,i) )  {
         fur->x = m3dPolyVertexX(poly,i);
      }
      if (fur->y < m3dPolyVertexY(poly,i) )  {
         fur->y = m3dPolyVertexY(poly,i);
      }
      if (fur->z < m3dPolyVertexZ(poly,i) )  {
         fur->z = m3dPolyVertexZ(poly,i);
      }
   }

   return;
}

/*****************************************************************
* m2dGetPolyBBox()
*
* Compute the bbox of the polygon in 2D
*
* IN:  poly      - input polygon
* OUT: bll, fur  - bbox corners
*****************************************************************/
void m2dGetPolyBBox (m2dPOLY *poly, m2dV *bll, m2dV *fur)
{
   int i;

   bll->x = bll->y =  M3D_INFINITY;
   fur->x = fur->y = -M3D_INFINITY;

   for (i = 0; i < m3dPolyNVert(poly); i++ )  {
      if (bll->x > m3dPolyVertexX(poly,i) )  {
         bll->x = m3dPolyVertexX(poly,i);
      }
      if (bll->y > m3dPolyVertexY(poly,i) )  {
         bll->y = m3dPolyVertexY(poly,i);
      }

      if (fur->x < m3dPolyVertexX(poly,i) )  {
         fur->x = m3dPolyVertexX(poly,i);
      }
      if (fur->y < m3dPolyVertexY(poly,i) )  {
         fur->y = m3dPolyVertexY(poly,i);
      }
   }

   return;
}

/*************************************************************
* m3dCalcTriArea2()
*
* Calculate triangle's area (using Heron's formulae)
* IN : p1, p2, p3 - triangle vertices
* RET: triangle area
*************************************************************/
float m3dCalcTriArea2 (m3dV *p1, m3dV *p2, m3dV *p3)
{
   m3dV v12, v13, cross;

   m3dSubtractVector(p2, p1, &v12);
   m3dSubtractVector(p3, p1, &v13);

   m3dCrossProduct(&v12, &v13, &cross);

   return 0.25f * m3dLengthVector_2(&cross);
}

/*************************************************************
* m2dCalcTriArea2()
*
* Calculate triangle's area (using Heron's formulae)
* IN : p1, p2, p3 - triangle vertices
* RET: triangle area
*************************************************************/
float m2dCalcTriArea2 (m2dV *p1, m2dV *p2, m2dV *p3)
{
   float s2;

   s2 = (p2->x - p1->x) * (p3->y - p1->y) - 
        (p3->x - p1->x) * (p2->y - p1->y);
   s2 = 0.25f*s2*s2;
   return s2;
}

/*************************************************************
* m2dCalcPolyArea()
*
* Calculate the area of CONVEX polygon
*  (subdividing it into triangles)
*
* IN:  poly - 3D polygon
* RET: polygon's area
*
* NOTE: for correct calculating the polygon MUST be convex
*************************************************************/
float m2dCalcPolyArea (m2dPOLY *poly)
{
   float area = 0.f;
   int   nv;

   for (nv = 1; nv <= m3dPolyNVert(poly)-2; nv++) {
      area += (float)m2dCalcTriArea(m3dPolyVertex(poly,0   ), 
                                    m3dPolyVertex(poly,nv  ),
                                    m3dPolyVertex(poly,nv+1));
   }

   return area;
}

/*************************************************************
* m3dCalcPolyArea()
*
* Calculate the area of CONVEX polygon
*  (subdividing it into triangles)
*
* IN:  poly - 3D polygon
* RET: polygon's area
*
* NOTE: for correct calculating the polygon MUST be convex
*************************************************************/
float m3dCalcPolyArea (m3dPOLY *poly)
{
   float area = 0.f;
   int   nv;

   for (nv = 1; nv <= m3dPolyNVert(poly)-2; nv++) {
      area += (float)m3dCalcTriArea(m3dPolyVertex(poly,0), m3dPolyVertex(poly,nv),m3dPolyVertex(poly,nv+1));
   }

   return area;
}

/*************************************************************
* m2dCalcPolyMassCenter() 
*
* Calculate the center (mass center, not centroid !!!) of a
* 2d convex polygon.
*
* IN:  poly   - polygon
* OUT: center - pointer to the result
**************************************************************/
void m2dCalcPolyMassCenter(m2dPOLY* poly, m2dV* center) 
{
   int    nv;
   m2dV   vSum, v;
   float  sum_area, area;

   ASSERT(m3dPolyNVert(poly)>=1);

   if (m3dPolyNVert(poly) == 3) {
      m2dAddVector(m3dPolyVertex(poly,0), m3dPolyVertex(poly,1), &vSum);
      m2dAddVector(&vSum, m3dPolyVertex(poly,2), &vSum);
      m2dScaleVector(&vSum, 1.f/3.f, center);
      return;
   }

   vSum = m2dVZero;
   sum_area = 0.f;

   for(nv = 1; nv < m3dPolyNVert(poly)-1; nv++) {
      m2dAddVector(m3dPolyVertex(poly,0), m3dPolyVertex(poly,nv), &v);
      _m2dAddVector(&v, m3dPolyVertex(poly,nv+1));
      _m2dScaleVector(&v, 1.f/3);
      area = (float)m2dCalcTriArea(m3dPolyVertex(poly,0), m3dPolyVertex(poly,nv), m3dPolyVertex(poly,nv+1));
      sum_area += area;
      
      _m2dScaleVector(&v, area);
      _m2dAddVector(&vSum, &v);
   }
   ASSERT(!m3dIsZero_A(sum_area, M3D_EPSILON/100));
   m2dScaleVector(&vSum, 1.f/sum_area, center);
   return;
}
/*************************************************************
* m3dCalcPolyMassCenter() 
*
* Calculate the center (mass center, not centroid !!!) of a
* 3d convex polygon.
*
* IN:  poly   - polygon
* OUT: center - pointer to the result
**************************************************************/
void m3dCalcPolyMassCenter (m3dPOLY* poly, m3dV* center) 
{
   int    nv;
   m3dV   vSum, v;
   float  sum_area, area;

   ASSERT(m3dPolyNVert(poly)>=1);

   if (m3dPolyNVert(poly) == 3) {
      m3dAddVector(m3dPolyVertex(poly,0), m3dPolyVertex(poly,1), &vSum);
      m3dAddVector(&vSum, m3dPolyVertex(poly,2), &vSum);
      m3dScaleVector(&vSum, 1.f/3.f, center);
      return;
   }

   vSum = m3dVZero;
   sum_area = 0.f;

   for (nv = 1; nv < m3dPolyNVert(poly)-1; nv ++) {
      m3dAddVector(m3dPolyVertex(poly,0), m3dPolyVertex(poly,nv), &v);
      _m3dAddVector(&v, m3dPolyVertex(poly,nv+1));
      _m3dScaleVector(&v, 1.f/3);
      area = (float)m3dCalcTriArea(m3dPolyVertex(poly,0), m3dPolyVertex(poly,nv), m3dPolyVertex(poly,nv+1));
      sum_area += area;
      
      _m3dScaleVector(&v, area);
      _m3dAddVector(&vSum, &v);
   }
   ASSERT(!m3dIsZero_A(sum_area, M3D_EPSILON/100));
   m3dScaleVector(&vSum, 1.f/sum_area, center);
   return;
}

/********************************************************
* m3dCalcPolyPerimeter()
*
* IN : pPoly - 3D polygon
* RET: perimeter
********************************************************/
float m3dCalcPolyPerimeter (m3dPOLY *pPoly)
{
   float dist, perimeter;
   int   nv,NVert;

   NVert     = m3dPolyNVert(pPoly);
   perimeter = 0.f;

   for (nv=0; nv<m3dPolyNVert(pPoly); nv++) {
      dist = m3dDist(m3dPolyVertex(pPoly,nv % NVert), m3dPolyVertex(pPoly, (nv+1) % NVert));
      perimeter += dist;
   }
   return perimeter;
}

/********************************************************
* m2dCalcPolyPerimeter()
*
* IN : pPoly - 2D polygon
* RET: perimeter
********************************************************/
float m2dCalcPolyPerimeter (m2dPOLY *pPoly)
{
   float dist, perimeter;
   int   nv, NVert;

   NVert     = m3dPolyNVert(pPoly);
   perimeter = 0.f;

   for (nv=0; nv<m3dPolyNVert(pPoly); nv++) {
      dist = m2dDist(m3dPolyVertex(pPoly,nv % NVert), m3dPolyVertex(pPoly, (nv+1) % NVert));
      perimeter += dist;
   }
   return perimeter;
}

/*****************************************************
* m2dGetPolyDiameter()
*
* Get polygon diameter (max distance between 
* polygon points)
*
* IN:  2D polygon
* OUT: diameter (line segment)
* RET: diameter length
*****************************************************/
float m2dGetPolyDiameter (m2dPOLY *poly2D, m2dV *lB, m2dV *lE) 
{
   int    i, j;
   float  dist2, maxDist2;

   ASSERT(poly2D);
   ASSERT((lB && lE) || (!(lB || lE)));

   maxDist2 = 0.f;
   for (i = 0; i < m3dPolyNVert(poly2D); i ++ ) {
      for (j = i+1; j < m3dPolyNVert(poly2D); j ++ ) {
         dist2 = m2dDist_2(m3dPolyVertex(poly2D,i), m3dPolyVertex(poly2D,j));
         if (dist2 > maxDist2) {
            maxDist2 = dist2;
            if (lB && lE) {
               *lB = *m3dPolyVertex(poly2D,i);
               *lE = *m3dPolyVertex(poly2D,j);
            }
         }
      }
   }
   return (float)m3dSqrt(maxDist2);
}

/*******************************************************************
* m2dReOrientPoly()
*
* IN:  poly - polygon
* OUT: poly - re-oriented polygon 
*
* Change the orientation of polygon. Non-degenerate polygon 
* must remain non-degenerate, therefore vertex indices must be 
* changed in the following way:
*
*  old new      | This is very important, because polygon projection
*   2   0       | matrix is computed using vertices 0,1,2
*   1   1       |
*   0   2       |
*  n-1  3       |
*  .......      |
*   3  n-1      |
*
*******************************************************************/
void m2dReOrientPoly (m2dPOLY *poly)
{
   m2dPOLY  tmpPoly;
   int      nvert, i;

   ASSERT(m3dPolyNVert(poly) >= 3);

   tmpPoly = *poly;
   nvert = m3dPolyNVert(poly);
   for (i = 0; i < nvert; i++) {
      switch(i) {
         case 0: 
            tmpPoly.vert[0] = poly->vert[2];
            break;
         case 1:
            tmpPoly.vert[1] = poly->vert[1];
            break;
         case 2:
            tmpPoly.vert[2] = poly->vert[0];
            break;
         default:
            tmpPoly.vert[i] = poly->vert[nvert+2-i];
      }
   }
   *poly = tmpPoly;
   return;
}

/*******************************************************************
* m3dReOrientPoly()
*
* IN:  poly - polygon
* OUT: poly - re-oriented polygon 
*
* Change the orientation of polygon. Non-degenerate polygon 
* must remain non-degenerate, therefore vertex indices must be 
* changed in the following way:
*
*  old new      | This is very important, because polygon projection
*   2   0       | matrix is computed using vertices 0,1,2
*   1   1       |
*   0   2       |
*  n-1  3       |
*  .......      |
*   3  n-1      |
*******************************************************************/
void m3dReOrientPoly (m3dPOLY *poly)
{
   m3dPOLY  tmpPoly;
   int      nvert, i;

   ASSERT(m3dPolyNVert(poly) >= 3);

   tmpPoly = *poly;
   nvert = m3dPolyNVert(poly);
   for (i = 0; i < nvert; i++) {
      switch(i) {
         case 0: 
            tmpPoly.vert[0] = poly->vert[2];
            break;
         case 1:
            tmpPoly.vert[1] = poly->vert[1];
            break;
         case 2:
            tmpPoly.vert[2] = poly->vert[0];
            break;
         default:
            tmpPoly.vert[i] = poly->vert[nvert+2-i];
      }
   }
   *poly = tmpPoly;
   return;
}

/*************************************************************
* BOOL m2dOrientCCWPoly()
*
* IN: poly2D - polygon
*
* Make poly2D a CCW-oriented polygon. To do this we check if
* the 3rd vertex lies to the left from edge (0-1). If vertex 2
* lies to the right => poly2D seems to be CW-oriented. We
* revert the order of vertices in it and ASSERT that 
* resultant poly2D is CCW-oriented
*************************************************************/
BOOL m2dOrientCCWPoly (m2dPOLY *poly2D)
{
   int   qual;

   qual = m2dQualPointLine(m3dPolyVertex(poly2D,2),
                           m3dPolyVertex(poly2D,0),
                           m3dPolyVertex(poly2D,1));

   // check that polygon is not degenerate
   ASSERT(qual == M3D_LAY_IN || qual == M3D_LAY_OUT );

   if (qual == M3D_LAY_OUT) {
      // seems that polygon is CW-oriented
      // its vertices can be not oriented at all - this is checked by 
      // ASSERT at the end
      // revert the order of polygon vertices
      m2dReOrientPoly(poly2D);
      return TRUE;
   }

#ifdef CHECK_CONVEX
   ASSERT(m2dIsPolyConvexCCW(poly2D));
#endif

   return FALSE;
}

/*************************************************************
* m2dIsPolyConvexCCW()
*
* IN:  poly  - polygon
* RET: TRUE  - polygon is convex
*      FALSE - polygon is concave
*
* Check if the poly2D is convex. If 'poly' is CW oriented,
* it is assumed to be concave
*************************************************************/
BOOL m2dIsPolyConvexCCW (const m2dPOLY *poly)
{
   int i;

   for (i = 0; i < poly->nVert; i ++)  {
      if (m2dQualPointLine_A(m3dPolyVertex(poly,(i+2) % poly->nVert),
                             m3dPolyVertex(poly,(i+0) % poly->nVert),
                             m3dPolyVertex(poly,(i+1) % poly->nVert),
                             10*M3D_EPSILON) == M3D_LAY_OUT )  {
         // if 3 points lie on the same line => polygon is not simple
         return FALSE;
      }
   }
   return TRUE;
}

/**********************************************************************
* m3dIsectRayPlane()
*
* IN:  rayOrg, rayDir - ray dierction
*      poly           - polygon in WCS, specifying the plane of interest
*      norm           - polygon normal (can be omitted)
* OUT: v              - intersection point
*      dist           - distance from 'rayOrg' to 'v' measured
*                       in 'ray units' (i.e., if 'rayDir' is normalized,
*                       than it is equal to ordinal distance)
* RET: TRUE           - intersection exists, FALSE - otherwise
* 
* Intersect the given ray and the plane specified by a poly
* Plane is treated as a 'directed' plane (it has a backface)
* NOTE: Formula is taken from Bronshtein, p. 225
**********************************************************************/
BOOL m3dIsectRayPlane (m3dV *rayOrg, m3dV *rayDir, m3dPOLY *poly, 
                       m3dV *v, m3dV *norm, float *dist)
{
   float  d;     // plane free coefficient
   float  rho;   // coefficient
   float  prod;
   m3dV   normTmp;

   if (!norm) {
      norm = &normTmp;
      m3dGetPolyNormal(poly, norm);
   }
   
   // check that normal is non-zero
   ASSERT(!m3dIsZero_A(m3dLengthVector(norm), 10.f*M3D_EPSILON));

   prod = m3dDotProduct(norm, rayDir);
   if (prod > -M3D_EPSILON)  {
      // cull backfaces and near-perpendicular planes
      return FALSE;
   }

   d = -m3dDotProduct(norm,m3dPolyVertex(poly,0));
   rho = (m3dDotProduct(norm, rayOrg) + d) / prod;

   if (rho > 0.f)  {
      return FALSE;
   }

   if (v) {
      v->x = rayOrg->x - rayDir->x*rho;
      v->y = rayOrg->y - rayDir->y*rho;
      v->z = rayOrg->z - rayDir->z*rho;
   }

   if (dist) {
      *dist = -rho;
   }

   return TRUE;
}

/**********************************************************************
* m3dIsectRayVNPlane()
*
* IN:  rayOrg, rayDir - ray dierction
*      point          - point on the plane
*      norm           - polygon normal
* OUT: v              - intersection point
*      dist           - distance from 'rayOrg' to 'v' measured
*                       in 'ray units' (i.e., if 'rayDir' is normalized,
*                       than it is equal to ordinal distance)
* RET: TRUE           - intersection exists, FALSE - otherwise
* 
* Intersect the given ray and the plane specified by a point and normal
* Plane is treated as a 'directed' plane (it has a backface)
* NOTE: Formula is taken from Bronshtein, p. 225
**********************************************************************/
BOOL m3dIsectRayVNPlane (const m3dV *rayOrg, const m3dV *rayDir, const m3dV *point, const m3dV *norm, m3dV *v, float *dist)
{
   float  d;     // plane free coefficient
   float  rho;   // coefficient
   float  prod;

   // check that normal is non-zero
   ASSERT(!m3dIsZero_A(m3dLengthVector(norm),10*M3D_EPSILON));

   prod = m3dDotProduct(norm,rayDir);
   if (prod > -M3D_EPSILON )  {
      // cull backfaces and near-perpendicular planes
      return FALSE;
   }

   d = -m3dDotProduct(norm,point);
   rho = (m3dDotProduct(norm,rayOrg) + d) / prod;

   if (rho > 0.f)  {
      return FALSE;
   }

   if (v)  {
      v->x = rayOrg->x - rayDir->x*rho;
      v->y = rayOrg->y - rayDir->y*rho;
      v->z = rayOrg->z - rayDir->z*rho;
   }

   if (dist)  {
      *dist = -rho;
   }

   return TRUE;
}
/**********************************************************************
* m3dIsectRayVNPlane()
*
* IN:  rayOrg, rayDir - ray dierction
*      point          - point on the plane
*      norm           - polygon normal
* OUT: v              - intersection point
*      dist           - distance from 'rayOrg' to 'v' measured
*                       in 'ray units' (i.e., if 'rayDir' is normalized,
*                       than it is equal to ordinal distance)
* RET: TRUE           - intersection exists, FALSE - otherwise
* 
* Intersect the given ray and the plane specified by a point and normal
* Plane is treated as a 'not directed' plane
* NOTE: Formula is taken from Bronshtein, p. 225
**********************************************************************/
BOOL _m3dIsectRayVNPlane (m3dV *rayOrg, m3dV *rayDir, m3dV *point, m3dV *norm, m3dV *v, float *dist)
{
   float  d;     // plane free coefficient
   float  rho;   // coefficient
   float  prod;

   // check that normal is non-zero
   ASSERT(!m3dIsZero_A(m3dLengthVector(norm),10*M3D_EPSILON));

   prod = m3dDotProduct(norm,rayDir);
   if (m3dIsZero(prod))  {
      // cull backfaces and near-perpendicular planes
      return FALSE;
   }

   d   = -m3dDotProduct(norm,point);
   rho = (m3dDotProduct(norm,rayOrg) + d) / prod;

   if (rho > 0.f)  {
      return FALSE;
   }

   if (v)  {
      _m3dLineCombine(rayOrg, rayDir, -rho, v);
   }

   if (dist)  {
      *dist = -rho;
   }
   return TRUE;
}
/********************************************************************
* m3dIsectRayTri()
*
* IN:  rayOrg, rayDir - ray
*      v0, v1, v2     - triangle
* OUT: vOrg - point on the intersection line
*      vDir - direction of the intersection line
* RET: FALSE if no intersection were found
********************************************************************/
BOOL m3dIsectRayTri (m3dV *rayOrg, m3dV *rayDir, m3dV *v0, m3dV *v1, m3dV *v2, float *dist)
{
   m3dV   edge1, edge2, tvec, pvec, qvec;
   float  u, v, det, inv_det;

   // find vectors for two edges sharing vert0 
   m3dSubtractVector(v1, v0, &edge1);
   m3dSubtractVector(v2, v0, &edge2);

   // begin calculating determinant - also used to calculate U parameter 
   m3dCrossProduct(rayDir, &edge2, &pvec);

   // if determinant is near zero, ray lies in plane of triangle 
   det = m3dDotProduct(&edge1, &pvec);

   if (det < M3D_EPSILON) {
      // backface culling
      return FALSE;
   }
   // calculate distance from vert0 to ray origin 
   m3dSubtractVector(rayOrg, v0, &tvec);

   // calculate U parameter and test bounds 
   u = m3dDotProduct(&tvec, &pvec);
   if (u < 0.0 || u > det) {
      return FALSE;
   }
   // prepare to test V parameter 
   m3dCrossProduct(&tvec, &edge1, &qvec);

   // calculate V parameter and test bounds 
   v = m3dDotProduct(rayDir, &qvec);
   if (v < 0.0 || u + v > det) {
      return FALSE;
   }
   inv_det = 1.f / det;

   // calculate dist, ray intersects triangle 
   *dist = m3dDotProduct(&edge2, &qvec) * inv_det;
   return TRUE;
}
/********************************************************************
* _m3dIsectRayTri()
*
* IN:  rayOrg, rayDir - ray
*      v0, v1, v2     - triangle
* OUT: dist - distance along the ray to intersection poit
* RET: FALSE if no intersection were found
********************************************************************/
BOOL _m3dIsectRayTri (m3dV *rayOrg, m3dV *rayDir, m3dV *v0, m3dV *v1, m3dV *v2, float *dist, BOOL *isBF)
{
   m3dV   edge1, edge2, tvec, pvec, qvec;
   float  u, v, det, inv_det;

   // find vectors for two edges sharing vert0 
   m3dSubtractVector(v1, v0, &edge1);
   m3dSubtractVector(v2, v0, &edge2);

   // begin calculating determinant - also used to calculate U parameter 
   m3dCrossProduct(rayDir, &edge2, &pvec);

   // if determinant is near zero, ray lies in plane of triangle 
   det = m3dDotProduct(&edge1, &pvec);

   if (det > M3D_EPSILON) {
      // calculate distance from vert0 to ray origin 
      m3dSubtractVector(rayOrg, v0, &tvec);

      // calculate U parameter and test bounds 
      u = m3dDotProduct(&tvec, &pvec);
      if (u < 0.0 || u > det) {
         return FALSE;
      }
      // prepare to test V parameter 
      m3dCrossProduct(&tvec, &edge1, &qvec);

      // calculate V parameter and test bounds 
      v = m3dDotProduct(rayDir, &qvec);
      if (v < 0.0 || u + v > det) {
         return FALSE;
      }
      *isBF = FALSE;
   } else if (det < -M3D_EPSILON) {
      // calculate distance from vert0 to ray origin 
      m3dSubtractVector(rayOrg, v0, &tvec);

      // calculate U parameter and test bounds 
      u = m3dDotProduct(&tvec, &pvec);
      if (u > 0.0 || u < det) {
         return FALSE;
      }
      // prepare to test V parameter 
      m3dCrossProduct(&tvec, &edge1, &qvec);

      // calculate V parameter and test bounds 
      v = m3dDotProduct(rayDir, &qvec) ;
      if (v > 0.0 || u + v < det) {
         return 0;
      }
      *isBF = TRUE;
   } else {
      return FALSE;  // ray is parallell to the plane of the triangle 
   }
   inv_det = 1.f / det;

   // calculate dist, ray intersects triangle 
   *dist = m3dDotProduct(&edge2, &qvec) * inv_det;
   return TRUE;
}

/*************************************************************
*  m3dIsectRayDomPoly()
*
* IN:  rayOrg, rayDir - ray
*      poly           - 2d polygon
       yLo, yHi       - bottom and top planes of the domain
* OUT: dist - distance along the ray to intersection poit
* RET: FALSE if no intersection were found
*************************************************************/
BOOL m3dIsectRayDomPoly(m3dV *rayOrg, m3dV *rayDir, m2dPOLY *poly, float yLo, float yHi, float *dist)
{
   float minDist = 1E20f;
   m3dV  v3d;
   m2dV  v2d;

   if (!m3dIsZero(rayDir->y)) {
      if (rayDir->y > 0.f) {
         if (rayOrg->y < yLo) {
            float k = (rayOrg->y - yLo) / rayDir->y;
            _m3dLineCombine(rayOrg, rayDir, k, &v3d);
            m3xzV3DToV2D(&v3d, &v2d);
            if (m2dIsBelongPointPoly(poly, &v2d)) {
               minDist = k;
            }
         }
      } else {
         if (rayOrg->y > yHi) {
            float k = (rayOrg->y - yLo) / rayDir->y;
            _m3dLineCombine(rayOrg, rayDir, k, &v3d);
            m3xzV3DToV2D(&v3d, &v2d);
            if (m2dIsBelongPointPoly(poly, &v2d)) {
               minDist = k;
            }
         }
      }
   }

   m3xzV3DToV2D(rayDir, &v2d);

   if (!m2dIsVZero(&v2d)) {
      for (int i = 0; i < poly->nVert; i++) {
         m2dV    *a = poly->vert + i, *b = poly->vert + (i + 1) % poly->nVert;

         if (m3dIsZero(m2dDist(a, b))) {
            continue;
         }

         m3dPOLY  plane;
         plane[0].x = a->x; plane[0].y = yLo; plane[0].z = a->y;
         plane[1].x = b->x; plane[1].y = yLo; plane[1].z = b->y;
         plane[2].x = a->x; plane[2].y = yHi; plane[2].z = a->y;
         plane.nVert = 3;

         m3dV  isect;
         float dist;
         if (m3dIsectRayPlane(rayOrg, rayDir, &plane, &isect, NULL, &dist)) {
            if (dist < minDist && isect.y > yLo - M3D_EPSILON && isect.y < yHi + M3D_EPSILON) {
               m2dV dir, p, isect2d;
               m3xzV3DToV2D(&isect, &isect2d);
               m2dSubtractVector(b, a, &dir);
               m2dSubtractVector(&isect2d, a, &p);
               if (m2dDotProduct(&dir, &p) < 0) {
                  continue;
               }
               m2dSubtractVector(a, b, &dir);
               m2dSubtractVector(&isect2d, b, &p);
               if (m2dDotProduct(&dir, &p) < 0) {
                  continue;
               }

               minDist = dist;
            }
         }
      }
   }

   *dist = minDist;

   return minDist < 1E10f;
}

/********************************************************************
* m3dIsectPlanePlane()
*
* IN:  v1 - vector on the 1st plane
*      n1 - normal to the 1st plane
*      v2 - vector on the 2nd plane
*      n2 - normal to the 2nd plane
* OUT: vOrg - point on the intersection line
*      vDir - direction of the intersection line
* RET: FALSE if planes are parallel
********************************************************************/
BOOL m3dIsectPlanePlane (m3dV *v1, m3dV *n1, m3dV *v2, m3dV *n2, m3dV *vOrg, m3dV *vDir)
{
   m3dMATR matr, matr_inv;
   m3dV    v3, n3;
   m3dV    w;

   // check that normals are indeed normal
   ASSERT(m3dIsZero_A(m3dLengthVector(n1)-1.f,10*M3D_EPSILON));
   ASSERT(m3dIsZero_A(m3dLengthVector(n2)-1.f,10*M3D_EPSILON));

   m3dCrossProduct(n1, n2, &n3);
   
   if(m3dLengthVector(&n3) < 100.f*M3D_EPSILON) {
      return FALSE; // planes are parallel
   }

   m3dNormalize(&n3);
   m3dMakeVZero(&v3);

   matr.Identity();
   matr.ElementRC(0,0) = n1->x;
   matr.ElementRC(1,0) = n1->y;
   matr.ElementRC(2,0) = n1->z;
   matr.ElementRC(0,1) = n2->x;
   matr.ElementRC(1,1) = n2->y;
   matr.ElementRC(2,1) = n2->z;
   matr.ElementRC(0,2) = n3.x ;
   matr.ElementRC(1,2) = n3.y ;
   matr.ElementRC(2,2) = n3.z ;

   w.x = m3dDotProduct( n1 , v1);
   w.y = m3dDotProduct( n2 , v2);
   w.z = m3dDotProduct(&n3, &v3);

   matr.Invert(&matr_inv);
   matr_inv.TransformVector(&w, vOrg);
   *vDir = n3;

   return TRUE;
}

/************************************************************
* m2dDistPointPoly ()
*
* IN:  point       - a point
*      poly        - poly
* OUT: vClosest    - closest point on the polygon
* RET: dist        - distance between 'point' and 'poly'
*
* Calculate the distance between point and polygon.
* If point is belong to the polygon, then the distance
* is zero. Otherwise it is defined as the minimal distance 
* between point and all polygon sides
************************************************************/
float m2dDistPointPoly (m2dV *point, m2dPOLY *poly, m2dV *vClosest)
{
   
   m2dV     v, vMin;
   float    d_2, dMin_2;
   int      i;

   if (m2dIsBelongPointPoly(poly, point)) {
      *vClosest = *point;
      return 0.f;
   }
   
   dMin_2 = m2dDistPointLSeg_2(point, &poly->vert[poly->nVert-1], &poly->vert[0], &vMin, NULL);
   for (i = 0; i < poly->nVert-1; i ++)  {
      d_2 = m2dDistPointLSeg_2(point, &poly->vert[i], &poly->vert[i+1], &v, NULL);
      if (d_2 < dMin_2)  {
         dMin_2 = d_2;
         vMin = v;
      }
   }

   if (vClosest) {
      *vClosest = vMin;
   }
   return m3dSqrt(dMin_2);
}
/************************************************************
* m2dDistPointPoly ()
************************************************************/
float m2dDistPointTri (m2dV *point, m2dTRI *tri, m2dV *vClosest)
{
   m2dPOLY   poly;

   poly.nVert   = 3;
   poly.vert[0] = tri->v1;
   poly.vert[1] = tri->v2;
   poly.vert[2] = tri->v3;
   return m2dDistPointPoly(point, &poly, vClosest);
}


/************************************************************
* m3dDistPointPoly()
*
* IN:  point       - a point in WCS
*      poly        - poly
*      norm        - normal of 'poly'
*      isIncludeBF - if FALSE, than if 'point' is on the backface
*                              of 'poly', then FALSE is returned
*                    if TRUE, the distance is computed anyway
* OUT: vClosest    - closest point on the polygon
*      dist        - distance between 'point' and 'poly'
*      distAlpha   - if distance between 'point' and 'poly'
*                    is greater than 'distAlpha', then the actual
*                    distance (and closest point) is not computed
*                    and FALSE is returned
* RET: TRUE  - closest point on 'poly' is found and it is closer
*              to 'point' than 'distAlpha'
*      FALSE - otherwise
*
* Calculate the distance between point and polygon.
* If point is projected onto the polygon, then the distance
* is measured along the polygon normal. Otherwise it is
* defined as the minimal distance between point and all
* polygon sides (LSegs in 3D)
************************************************************/
BOOL m3dDistPointPoly (const m3dV *point, m3dPOLY *poly, m3dV *norm, BOOL isIncludeBF, m3dV *vClosest, float *dist, float distAlpha)
{
   m3dMATR  mat, imat;
   m3dV     tranPoint, v, vMin;
   m3dPOLY  tranPoly;
   float    d, d_2, dMin_2;
   int      i;
   m3dV     vx, vz;
   BOOL     rc;

#ifdef _DEBUG
   m3dV     polyNormal;

   // make sure poly normal is indeed poly normal
   m3dGetPolyNormal(poly, &polyNormal);
   ASSERT(m3dIsEqualPoint_A(&polyNormal, norm, 0.01f));
#endif

   // compute the distance from the point to the plane of polygon
   m3dSubtractVector(point, m3dPolyVertex(poly,0), &v);
   d = m3dDotProduct(&v, norm);
   if (d > distAlpha || d < - distAlpha) {
      // alpha-cut: point is far away from the plane of polygon
      return FALSE;
   }
   if (!isIncludeBF && d < -M3D_EPSILON) {
      return FALSE; // backface
   }

   // calculate the matrix of projection onto polygon
   m3dSubtractVector(m3dPolyVertex(poly,1), m3dPolyVertex(poly,0), &vx);
   if (!_m3dNormalize(&vx)) {
      // degenerate polygon - either line segment, or a point
      // check for point
      m3dSubtractVector(m3dPolyVertex(poly,2), m3dPolyVertex(poly,0), &vx);
      if (!_m3dNormalize(&vx)) {
         // polygon is a point
         d = m3dDist(m3dPolyVertex(poly,0), point);
         if (d >= distAlpha) {
            return FALSE;
         }
         if (dist) {
            *dist = d;
         }
         if (vClosest) {
            *vClosest = *m3dPolyVertex(poly,0);
         }
         return TRUE;
      }
      // polygon is a line segment
      return m3dDistPointLSeg(point, m3dPolyVertex(poly,0), m3dPolyVertex(poly,2), distAlpha, dist, vClosest);
   }
   m3dCrossProduct(&vx,norm,&vz);
   mat.MakeWCS2LCS(m3dPolyVertex(poly,0),&vx,norm,&vz);

   mat.TransformPoint(point,&tranPoint);
   // point lies on the backface side of the polygon
   ASSERT(isIncludeBF || tranPoint.y > -0.001f);

   mat.TransformPoly(poly,&tranPoly);

   if (m3xzIsBelongPointPoly(&tranPoly,&tranPoint) )  {
      // point is projected onto the surface of the polygon
      // the dist(point, poly) = dist(point, polyPlane)
#ifdef _DEBUG
      float _d;
      
      _d =  m3dDistPointPlane(point, poly, norm);
      _d -= m3dFAbs(tranPoint.y);
      ASSERT(m3dIsZero_A(_d,500*M3D_EPSILON));
#endif
      
      if (dist) {
         *dist = m3dFAbs(tranPoint.y);
      }
      if (vClosest) {
         tranPoint.y = 0.f;
         imat.MakeLCS2WCS(m3dPolyVertex(poly,0),&vx,norm,&vz);
         imat.TransformPoint(&tranPoint,vClosest);
      }

      return TRUE;
   }

   // point is not projected onto the surface of the polygon
   // calculate the minimal distance between the point and 
   // sides of polygon
   rc = m3xzDistPointLSeg_2(&tranPoint, m3dPolyVertex(&tranPoly,m3dPolyNVert(poly)-1), m3dPolyVertex(&tranPoly,0), M3D_INFINITY, &dMin_2, &vMin);
   ASSERT(rc == TRUE);
   for (i = 0; i < m3dPolyNVert(poly)-1; i++ )  {
      if (m3xzDistPointLSeg_2(&tranPoint, m3dPolyVertex(&tranPoly,i), m3dPolyVertex(&tranPoly,i+1), dMin_2, &d_2, &v)) {
         ASSERT(d_2 < dMin_2 + 0.01f);
         dMin_2 = d_2;
         vMin = v;
      }
   }

   // alpha-cut
   if (dMin_2 > m3dPow2(distAlpha))  {
      return FALSE;
   }

   d = m3dDist(&tranPoint, &vMin);
   if (d > distAlpha + 5.f*M3D_EPSILON)  {
      return FALSE;
   }

   if (dist) {
      *dist = d;
   }
   if (vClosest) {
      imat.MakeLCS2WCS(m3dPolyVertex(poly,0), &vx, norm, &vz);
      imat.TransformPoint(&vMin, vClosest);
   }

   return TRUE;
}
/************************************************************
* m3dDistPointTri_2 ()
* 
************************************************************/
BOOL m3dDistPointTri_2 (const m3dV *point, const m3dV *v0, const m3dV *v1, const m3dV *v2, BOOL isCullBF, float distAlphaCut_2, float *_dist_2, m3dV *vClosest)
{
   m3dV   edge0, edge1, vD;

   m3dSubtractVector(v0, point, &vD);
   m3dSubtractVector(v1, v0, &edge0);
   m3dSubtractVector(v2, v0, &edge1);

   if (isCullBF) {
      m3dV   perpVect;
      m3dCrossProduct(&edge0, &edge1, &perpVect);

      if (m3dDotProduct(&vD, &perpVect) > M3D_EPSILON) {
         return FALSE;
      }
   }
   
   float e0_2 = m3dLengthVector_2(&edge0);
   float e0e1 = m3dDotProduct(&edge0, &edge1);
   float e1_2 = m3dLengthVector_2(&edge1);
   float det  = m3dFAbs(e0_2*e1_2-e0e1*e0e1);
   if (det < M3D_EPSILON) {
      return FALSE; // degenerate triangle 
   }

   float e0vD  = m3dDotProduct(&vD, &edge0);
   float e1vD  = m3dDotProduct(&vD, &edge1);
   float vD_2   = m3dLengthVector_2(&vD);
   float s   = e0e1*e1vD-e1_2*e0vD;
   float t   = e0e1*e0vD-e0_2*e1vD;
   float tmp0, tmp1, num, denom;
   float dist_2;

   if (s + t > det) {
      if (s < 0.f) { // location 2
         tmp0 = e0e1   + e0vD;
         tmp1 = e1_2 + e1vD;
         if (tmp1 > tmp0) {
            num = tmp1 - tmp0;
            denom = e0_2-2.f*e0e1+e1_2;
            if (num >= denom) { s = 1.f; t = 0.f; dist_2 = e0_2+(2.f)*e0vD+vD_2; } else { s = num/denom; t = 1.f - s; dist_2 = s*(e0_2*s+e0e1*t+2.f*e0vD) + t*(e0e1*s+e1_2*t+(2.f)*e1vD)+vD_2; }
         } else {
            s = 0.f;
            if (tmp1 <= 0.f) { t = 1.f; dist_2 = e1_2+2.f*e1vD+vD_2; } else if (e1vD >= 0.f) { t = 0.f; dist_2 = vD_2; } else { t = -e1vD/e1_2; dist_2 = e1vD*t+vD_2; }
         }
      } else if (t < 0.f) { // location 6
         tmp0 = e0e1 + e1vD;
         tmp1 = e0_2 + e0vD;
         if (tmp1 > tmp0) {
            num   = tmp1 - tmp0;
            denom = e0_2-2.f*e0e1+e1_2;
            if (num >= denom) { t = 1.f; s = 0.f; dist_2 = e1_2+(2.f)*e1vD+vD_2; } else { t = num/denom; s  = 1.f - t; dist_2 = s*(e0_2*s+e0e1*t+(2.f)*e0vD) + t*(e0e1*s+e1_2*t+(2.f)*e1vD)+vD_2;}
         } else {
            t = 0.f;
            if (tmp1 <= 0.f) { s = 1.f; dist_2 = e0_2+(2.f)*e0vD+vD_2; } else if (e0vD >= 0.f) { s = 0.f; dist_2 = vD_2; } else { s = -e0vD/e0_2; dist_2 = e0vD*s+vD_2; }
         }
      } else { // location 1
         num = e1_2 + e1vD - e0e1 - e0vD;
         if (num <= 0.f) {
            s      = 0.f;
            t      = 1.f;
            dist_2 = e1_2+(2.f)*e1vD+vD_2;
         } else {
            denom = e0_2-2.f*e0e1+e1_2;
            if (num >= denom) { s = 1.f; t = 0.f; dist_2 = e0_2+(2.f)*e0vD+vD_2; } else { s = num/denom; t = 1.f - s; dist_2 = s*(e0_2*s+e0e1*t+(2.f)*e0vD) + t*(e0e1*s+e1_2*t+(2.f)*e1vD)+vD_2; }
         }
      }
   } else {
      if (s < 0.f) {
         if (t < 0.f) { // location 4
            if (e0vD < 0.f){
               t = 0.f;
               if (-e0vD >= e0_2) { s = 1.f; dist_2 = e0_2+2.f*e0vD+vD_2; } else { s = -e0vD/e0_2; dist_2 = e0vD*s+vD_2; }
            } else {
               s = 0.0;
               if (e1vD  >= 0.f) { t = 0.f; dist_2 = vD_2;} else if (-e1vD >= e1_2) { t = 1.f; dist_2 = e1_2+2.f*e1vD+vD_2; } else { t = -e1vD/e1_2; dist_2 = e1vD*t+vD_2; }
            }
         } else { // location 3
            s = 0.f;
            if (e1vD >= 0.f) { t = 0.f; dist_2 = vD_2; } else if (-e1vD >= e1_2) { t = 1.f; dist_2 = e1_2+2.f*e1vD+vD_2; } else { t = -e1vD/e1_2; dist_2 = e1vD*t+vD_2; }
         }
      } else if (t < 0.f) { // location 5
         t = 0.f;
         if (e0vD >= 0.f) { s = 0.f; dist_2 = vD_2; } else if (-e0vD >= e0_2) { s = 1.f; dist_2 = e0_2+2.f*e0vD+vD_2; } else { s = -e0vD/e0_2; dist_2 = e0vD*s+vD_2; }
      } else { // location 0
         // minimum inside
         float detInv = (1.f)/det;
         s *= detInv;
         t *= detInv;
         dist_2 = s*(e0_2*s+e0e1*t+(2.f)*e0vD) + t*(e0e1*s+e1_2*t+(2.f)*e1vD)+vD_2;
      }
   }
   *_dist_2 = m3dFAbs(dist_2);

   if (dist_2 > distAlphaCut_2) {
      return FALSE;
   }
   if (vClosest) {
      m3dLineCombine(&edge0, s, &edge1, t, vClosest);
      _m3dAddVector(vClosest, v0);
   }
   return TRUE;
}
/************************************************************
* m3dDistPointTri ()
************************************************************/
BOOL m3dDistPointTri (const m3dV *point, const m3dV *v0, const m3dV *v1, const m3dV *v2, BOOL isCullBF, float distAlphaCut, float *dist, m3dV *vClosest)
{
   BOOL rc;
   if (rc = m3dDistPointTri_2(point, v0, v1, v2, isCullBF, m3dPow2(distAlphaCut), dist, vClosest)) {
      *dist = m3dSqrt(*dist);
   }
   return rc;
}
/************************************************************
* m3dDistLSegPoly()
*
* Calculate the distance between line segment and polygon.
*
* IN:  lB         - start point of line segment
*      lE         - end point of line segment
*      poly       - poly
*      norm       - normal of 'poly'
*      distAlpha  - if distance between 'lseg' and 'poly'
*                   is greater than 'distAlpha', then the actual
*                   distance (and closest point) is not computed
*                   and FALSE is returned
* OUT: vClosLSeg  - closest point on the line segment
*      vClosPoly  - closest point on the polygon
*      minDist    - minimal distance between objects
*      indEdge    - index of the closest edge on the polygon
* RET: TRUE  - closest point on 'poly' is found and it is closer
*              to 'lSeg' than 'distAlpha'
*      FALSE - otherwise
************************************************************/
BOOL m3dDistLSegPoly (m3dV *lB, m3dV *lE, m3dPOLY *poly, m3dV *norm, m3dV *vClosLSeg, m3dV *vClosPoly, float *minDist, int *indEdge, float distAlpha)
{
   m3dV   v1, v2, vLSeg, vPoly;
   float  dist, d1, d2;
   int    i, ind1, ind2;
   BOOL   fDist;

   d1 = m3dDistPointPlane_S(lB, m3dPolyVertex(poly, 0), norm);
   d2 = m3dDistPointPlane_S(lE, m3dPolyVertex(poly, 0), norm);
   if ((d1 > distAlpha && d2 > distAlpha) ||
       (d1 < -.05f && d2 < -0.05f)) {
      return FALSE;
   }
   
   if (m3dIsectLSegPoly(lB, lE, poly, norm, &v1)) {
      *vClosLSeg = *vClosPoly = v1;
      *minDist = 0.f;
      *indEdge = -1;
      return TRUE;
   }

   fDist = FALSE;
   // if poly/lseg are parallel, then the priority is given to edge-edge distance over vertex/face distance
   for (i = 0; i < m3dPolyNVert(poly); i++) {
      ind1 = i;
      ind2 = (i+1) % m3dPolyNVert(poly);
      if (m3dDistLSegLSeg(lB, lE, m3dPolyVertex(poly, ind1), m3dPolyVertex(poly, ind2), distAlpha, &dist, &v1, &v2)) {
         vLSeg     = v1;
         vPoly     = v2;
         distAlpha = dist;
         fDist     = TRUE;
         *indEdge  = i;
      }
   }
   if (m3dDistPointPoly(lB, poly, norm, TRUE, &vPoly, &dist, distAlpha)) {
      vLSeg     = *lB;
      distAlpha = dist;
      fDist     = TRUE;
      *indEdge = -1;
   }
   if (m3dDistPointPoly(lE, poly, norm, TRUE, &vPoly, &dist, distAlpha)) {
      vLSeg     = *lE;
      distAlpha = dist;
      fDist     = TRUE;
      *indEdge = -1;
   }
   if (fDist) {
      ASSERT(m3dIsZero_A(distAlpha - m3dDist(&vLSeg, &vPoly), 500.f*M3D_EPSILON));

      *vClosLSeg = vLSeg;
      *vClosPoly = vPoly;
      *minDist   = distAlpha;
   }
   return fDist;
}

/************************************************************
* m3dDistLSegPoly()
*
* Calculate the distance between line segment and polygon.
*
* IN:  lB         - start point of line segment
*      lE         - end point of line segment
*      vert       - point on the plane
*      norm       - plane normal
*      distAlpha  - if distance between 'lseg' and 'poly'
*                   is greater than 'distAlpha', then the actual
*                   distance (and closest point) is not computed
*                   and FALSE is returned
* OUT: vClosLSeg  - closest point on the line segment
*      vClosPlane - closest point on the polygon
*      minDist    - minimal distance between objects
*      indEdge    - index of the closest edge on the polygon
* RET: TRUE  - closest point on 'poly' is found and it is closer
*              to 'lSeg' than 'distAlpha'
*      FALSE - otherwise
************************************************************/
BOOL m3dDistLSegPlane (m3dV *lB, m3dV *lE, m3dV *vert, m3dV *norm, m3dV *vClosLSeg, m3dV *vClosPlane, float *minDist, float distAlpha)
{
   m3dV   v;
   float  d1, d2;

   d1 = m3dDistPointPlane_S(lB, vert, norm);
   d2 = m3dDistPointPlane_S(lE, vert, norm);
   if ((d1 > distAlpha && d2 > distAlpha) ||
       (d1 < -100.f*M3D_EPSILON && d2 < -100.f*M3D_EPSILON)) {
      return FALSE;
   }
   
   if (m3dIsectLSegPlane(lB, lE, vert, norm, &v)) {
      *vClosLSeg = *vClosPlane = v;
      *minDist = 0.f;
      return TRUE;
   }

   if (min(d1, d2) > distAlpha) {
      return FALSE;
   }
   if (d1 < d2) {
      *vClosLSeg = *lB;
      m3dProjPointPlane(vert, norm, lB, vClosPlane);
      *minDist = d1;
   } else {
      *vClosLSeg = *lE;
      m3dProjPointPlane(vert, norm, lE, vClosPlane);
      *minDist = d2;
   }

   return TRUE;
}

/*********************************************************************
* m3dIsectLSegPlane()
*
* IN:  vB, vE  - line segment specification
*      poly    - polygon to intersect with
*      vNorm   - normal to polygon
* OUT: vIsect  - intersection point
* RET: TRUE  - intersection is detected
*      FALSE - otherwise
*
* Intersect line segment with polygon in 3D
*********************************************************************/
BOOL m3dIsectLSegPlane (m3dV *vB, m3dV *vE, m3dV *vert, m3dV *norm, m3dV *vIsect)
{
   m3dV  vDir, v;
   float len;
   float prod, d, rayT;

   m3dSubtractVector(vE, vB, &vDir);
   len = m3dLengthVector(&vDir);

   if (m3dIsZero(len)) {
      if (!m3dIsZero_A(m3dDistPointPlane(vB, vert, norm), 100 * M3D_EPSILON)) {
         return FALSE;
      }
      if (vIsect) {
         *vIsect = *vB;
      }
      return TRUE;
   }

   ASSERT(len > M3D_EPSILON);

   _m3dScaleVector(&vDir, 1/len);
   
   prod = m3dDotProduct(norm, &vDir);
   // performing backface culling
   if (m3dIsZero(prod)) {
      // TODO
      // check if line segment lies in poly 
      return FALSE;
   }

   // this code is pretty much the same as in m3dIsectRayVNPlane_T()
   d = -m3dDotProduct(norm, vert);
   rayT = -((m3dDotProduct(norm, vB) + d) / prod);

   if (rayT < 0.f)  {
      // ray does not intersect the plane of object face
      return FALSE;
   }

   if (rayT > len) {
      // this is intersection point lies farther away than
      // the end of the line segment
      return FALSE;
   }

   // compute the point of intersection
   v.x = vB->x + vDir.x*rayT;
   v.y = vB->y + vDir.y*rayT;
   v.z = vB->z + vDir.z*rayT;

   if (vIsect) {
      *vIsect = v;
   }
   return TRUE;
}

/*********************************************************************
* m3dIsectLSegPoly()
*
* IN:  vB, vE  - line segment specification
*      poly    - polygon to intersect with
*      vNorm   - normal to polygon
* OUT: vIsect  - intersection point
* RET: TRUE  - intersection is detected
*      FALSE - otherwise
*
* Intersect line segment with polygon in 3D
*********************************************************************/
BOOL m3dIsectLSegPoly (m3dV *vB, m3dV *vE, m3dPOLY *poly, m3dV *vNorm, m3dV *vIsect)
{
   m3dV  vDir, v;
   float len;
   float prod, d, rayT;

   m3dSubtractVector(vE, vB, &vDir);
   len = m3dLengthVector(&vDir);

   if (m3dIsZero(len)) {
      if (!m3dIsZero_A(m3dDistPointPlane(vB, poly), 100 * M3D_EPSILON)) {
         return FALSE;
      }
      if (!m3dIsBelongPointPoly(poly, vB, vNorm)) {
         return FALSE;
      }
      if (vIsect) {
         *vIsect = *vB;
      }
      return TRUE;
   }

   ASSERT(len > M3D_EPSILON);

   _m3dScaleVector(&vDir, 1/len);
   
   prod = m3dDotProduct(vNorm, &vDir);
   // performing backface culling
   if (m3dIsZero(prod)) {
      // TODO
      // check if line segment lies in poly 
      return FALSE;
   }

   // this code is pretty much the same as in m3dIsectRayVNPlane_T()
   d = -m3dDotProduct(vNorm, m3dPolyVertex(poly, 0));
   rayT = -((m3dDotProduct(vNorm, vB) + d) / prod);

   if (rayT < 0.f)  {
      // ray does not intersect the plane of object face
      return FALSE;
   }

   if (rayT > len) {
      // this is intersection point lies farther away than
      // the end of the line segment
      return FALSE;
   }

   // compute the point of intersection
   v.x = vB->x + vDir.x*rayT;
   v.y = vB->y + vDir.y*rayT;
   v.z = vB->z + vDir.z*rayT;

   // check if intersection point belongs to face
   if (!m3dIsBelongPointPoly(poly, &v, vNorm)) {
      return FALSE;
   }
   if (vIsect) {
      *vIsect = v;
   }
   return TRUE;
}

/*************************************************************
* m2dPolyGetBasisCoord ()
*
*************************************************************/
void m2dPolyGetBasisCoord (m2dPOLY *poly, m2dV *coord, float *kA, float *kB)
{  
   m2dV  vA, vB, vP;

   ASSERT(m2dIsBelongPointPoly(poly, coord));

   m2dSubtractVector(m3dPolyVertex(poly, 1), m3dPolyVertex(poly, 0), &vA);
   m2dSubtractVector(m3dPolyVertex(poly, 2), m3dPolyVertex(poly, 0), &vB);
   m2dSubtractVector(coord, m3dPolyVertex(poly, 0), &vP);
   m2dBasisCoord(&vA, &vB, &vP, kA, kB);

   return;
}
/*************************************************************
* m2dPolyApplyBasisCoord ()
*
*************************************************************/
void m2dPolyApplyBasisCoord (m2dPOLY *poly, float kA, float kB, m2dV *coord)
{  
   m2dV  vA, vB;

   m2dSubtractVector(m3dPolyVertex(poly, 1), m3dPolyVertex(poly, 0), &vA);
   m2dSubtractVector(m3dPolyVertex(poly, 2), m3dPolyVertex(poly, 0), &vB);
   m2dLineCombine(&vA, kA, &vB, kB, coord);
   _m2dAddVector(coord, m3dPolyVertex(poly, 0));

   return;
}
/*************************************************************
* m2dTriGetBasisCoord ()
*
*************************************************************/
void m2dTriGetBasisCoord (m2dTRI *poly, m2dV *coord, float *kA, float *kB)
{  
   m2dV  vA, vB, vP;

   // ASSERT(m2dIsBelongPointTri(poly, coord)); recursion :-)

   m2dSubtractVector(poly->GetVert(1), poly->GetVert(0), &vA);
   m2dSubtractVector(poly->GetVert(2), poly->GetVert(0), &vB);
   m2dSubtractVector(coord, poly->GetVert(0), &vP);
   m2dBasisCoord(&vA, &vB, &vP, kA, kB);

   return;
}
/*************************************************************
* m2dTriApplyBasisCoord ()
*
*************************************************************/
void m2dTriApplyBasisCoord (m2dTRI *poly, float kA, float kB, m2dV *coord)
{  
   m2dV  vA, vB;

   m2dSubtractVector(poly->GetVert(1), poly->GetVert(0), &vA);
   m2dSubtractVector(poly->GetVert(2), poly->GetVert(0), &vB);
   m2dLineCombine(&vA, kA, &vB, kB, coord);
   _m2dAddVector(coord, poly->GetVert(0));

   return;
}




/*************************************************************
* m3dPolyGetBasisCoord ()
*
*************************************************************/
void m3dPolyGetBasisCoord (m3dPOLY *poly, m3dV *coord, float *kA, float *kB)
{  
   m3dV  vA, vB, vP;

   ASSERT(m3dIsBelongPointPoly(poly, coord, NULL));

   m3dSubtractVector(m3dPolyVertex(poly, 1), m3dPolyVertex(poly, 0), &vA);
   m3dSubtractVector(m3dPolyVertex(poly, 2), m3dPolyVertex(poly, 0), &vB);
   m3dSubtractVector(coord, m3dPolyVertex(poly, 0), &vP);
   m3dBasisCoord(&vA, &vB, &vP, kA, kB);

   return;
}
/*************************************************************
* m3dPolyApplyBasisCoord ()
*
*************************************************************/
void m3dPolyApplyBasisCoord (m3dPOLY *poly, float kA, float kB, m3dV *coord)
{  
   m3dV  vA, vB;

   m3dSubtractVector(m3dPolyVertex(poly, 1), m3dPolyVertex(poly, 0), &vA);
   m3dSubtractVector(m3dPolyVertex(poly, 2), m3dPolyVertex(poly, 0), &vB);
   m3dLineCombine(&vA, kA, &vB, kB, coord);
   _m3dAddVector(coord, m3dPolyVertex(poly, 0));

   return;
}
/*************************************************************
'* m3dPOLY::GetSquare()
'* half the square of parallelogram
'* RET : the square [m*m]
'*************************************************************/
float m3dPOLY::GetSquare(void)
{
   m3dV  vA, vB, vCross;
   m3dSubtractVector(m3dPolyVertex(this, 1), m3dPolyVertex(this, 0), &vA);
   m3dSubtractVector(m3dPolyVertex(this, 2), m3dPolyVertex(this, 0), &vB);
   m3dCrossProduct(&vA, &vB, &vCross);
   float sq = m3dLengthVector(&vCross) * 0.5f;
   return sq;
}

/*************************************************************
* m3dTriGetBasisCoord ()
*
*************************************************************/
void m3dTriGetBasisCoord (m3dTRI *poly, m3dV *coord, float *kA, float *kB)
{  
   m3dV  vA, vB, vP;

   m3dSubtractVector(poly->GetVert(1), poly->GetVert(0), &vA);
   m3dSubtractVector(poly->GetVert(2), poly->GetVert(0), &vB);
   m3dSubtractVector(coord, poly->GetVert(0), &vP);
   m3dBasisCoord(&vA, &vB, &vP, kA, kB);

   return;
}
/*************************************************************
* m3dTriApplyBasisCoord ()
*
*************************************************************/
void m3dTriApplyBasisCoord (m3dTRI *poly, float kA, float kB, m3dV *coord)
{  
   m3dV  vA, vB;

   m3dSubtractVector(poly->GetVert(1), poly->GetVert(0), &vA);
   m3dSubtractVector(poly->GetVert(2), poly->GetVert(0), &vB);
   m3dLineCombine(&vA, kA, &vB, kB, coord);
   _m3dAddVector(coord, poly->GetVert(0));

   return;
}

/*************************************************************
 * m2dIsCrossLSegPolyBoundary_A()
 *
 * RET: TRUE => has an intersection
 * IN:  poly
 * IN:  v1, v2 - LSeg
 * IN:  accuracy
 *************************************************************/
BOOL m2dIsCrossLSegPolyBoundary_A(m2dPOLY *poly, m2dV *v1, m2dV *v2, float accuracy)
{
   int i;
   m2dV *prev, *cur;

   ASSERT(poly && v1 && v2);

   // for all segents in poly
   prev = poly->vert + poly->nVert-1;
   cur  = poly->vert;
   for (i=poly->nVert; --i>=0; ) {
      // any isect is enough
      if (m2dIsectLSegLSeg(prev, cur, v1, v2, NULL)) {
         return TRUE;
      }

      // next lseg in poly
      prev = cur++;
   }

   return FALSE;
}

//
// End-of-file M3D_POLY.CPP
//      
